﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using AMEE.MVVMBase;
using System.Windows.Media.Media3D;
using System.Windows.Input;
using AMEE.PlaneDetection;
using HelixToolkit.Wpf;
using Microsoft.Kinect;
using System.Windows.Media;
using System.Windows.Threading;
using Visualizer.HelixToolkitWpf;

namespace Visualizer.ViewModels
{
    /// <summary>
    /// Hält PlaneFinder und GUI-Eigenschaften.<para>
    /// Führt auf Befehl PlaneRansac aus und erzeugt zum Ergebnis ein <c>Model3D</c> (genauer 
    /// <c>Model3DGroup</c>).
    /// </para>
    /// </summary>
    class MainWindowVM : ViewModelBase
    {
        #region Fields

        Model3D _model3D;
        PlaneFinder _planeFinder;
        MainWindow _window;
        bool _isScanning = false;

        readonly string ReadyState = "Ready";
        readonly string ScanningState = "Scanning";
        readonly string NoPlaneState = "No Plane (Ready)";

        // Farben siehe http://msdn.microsoft.com/query/dev12.query?appId=Dev12IDEF1&l=EN-US&k=k(System.Windows.Media.Colors);k(TargetFrameworkMoniker-.NETFramework,Version%3Dv4.5);k(DevLang-csharp)&rd=true
        public static readonly Material GREENMaterial = MaterialHelper.CreateMaterial(Colors.Green);
        public static readonly Material SNOWWhiteMaterial = MaterialHelper.CreateMaterial(Colors.Snow);
        public static readonly Material GREYMaterial = MaterialHelper.CreateMaterial(Colors.Gainsboro);
        public static readonly Material REDMaterial = MaterialHelper.CreateMaterial(Colors.Red);
        public static readonly Material YELLOWMaterial = MaterialHelper.CreateMaterial(Colors.Yellow);
        public static readonly Material YELLOWLgtMaterial = MaterialHelper.CreateMaterial(Colors.LightYellow);

        static readonly Material[] MATERIALS;

        #endregion


        #region Properties

        ///// <summary>
        ///// Gets or sets the number of tiles.<para>
        ///// (Division of point cloud in zones)</para>
        ///// </summary>
        ///// <value>
        ///// The numbers of tiles (zones).
        ///// </value>
        //public int Tiles {
        //    get { return _tiles; }
        //    set {
        //        int oldVal = _tiles;
        //        if (value > 0) {
        //            _tiles = value;
        //            if (_tiles != oldVal) {
        //                base.OnPropertyChanged("Tiles");
        //            }
        //        }
        //    }
        //}
        //int _tiles;

        /// <summary>
        /// Gets or sets the number of row zones.<para>
        /// (Vertical division of point cloud in zones)</para>
        /// </summary>
        /// <value>
        /// The numbers of vertical zones.
        /// </value>
        public int Rows {
            get { return _rows; }
            set {
                int oldVal = _rows;
                if (value > 0) {
                    _rows = value;
                    if (_rows != oldVal) {
                        base.OnPropertyChanged("Rows");
                    }
                }
            }
        }
        int _rows;

        /// <summary>
        /// Gets or sets the number of column zones.<para>
        /// (Horizontal division of point cloud in zones)</para>
        /// </summary>
        /// <value>
        /// The numbers of horizontal zones.
        /// </value>
        public int Columns {
            get { return _cols; }
            set {
                int oldVal = _cols;
                if (value > 0) {
                    _cols = value;
                    if (_cols != oldVal) {
                        base.OnPropertyChanged("Columns");
                    }
                }
            }
        }
        int _cols;

        /// <summary>
        /// Gets the state.<para>
        /// GUI Anzeige, ob bspw. gerade gescannt wird oder Scan erfolglos war.
        /// </para>
        /// </summary>
        /// <value>
        /// The state.
        /// </value>
        public String State {
            get { return _state; }
            private set {
                _state = value == null ? "unknown" : value;
                base.OnPropertyChanged("State");
            }
        }
        string _state;

        /// <summary>
        /// Gets or sets the model3d which contains the Point Cloud mesh.
        /// </summary>
        /// <value>
        /// The model3d.
        /// </value>
        public Model3D Model3D {
            get {
                if (_model3D == null) {
                    _model3D = new Model3DGroup();
                }
                return _model3D;
            }
            set {
                if (value == null) {
                    return;
                }
                _model3D = value;
                base.OnPropertyChanged("Model3D");
            }
        }

        /// <summary>
        /// Gets or sets the minimum distance between a data point and
        /// the model used to decide whether the point is an inlier or not.
        /// </summary>
        /// <value>
        /// The threshold.
        /// </value>
        public double Threshold { 
            get {return _threshold;}
            internal set { // wird normalerweise nur von GUI aus gesetzt.
                if (_planeFinder != null) {
                    _planeFinder.RansacThreshold = value;
                    _threshold = _planeFinder.RansacThreshold;
                    if (value != _threshold) {
                        OnPropertyChanged("Threshold");
                    }
                }
                else {
                    _threshold = value;
                }
            } 
        }
        double _threshold;

        /// <summary>
        /// W.keit, dass mindesten eine Punktmenge ohne Ausreißer gefunden wird. (?)
        /// </summary>
        /// <value>
        /// The probability.
        /// </value>
        public double Probability {
            get { return _probability; }
            internal set { // wird normalerweise nur von GUI aus gesetzt.
                if (_planeFinder != null) {
                    _planeFinder.RansacProbability = value;
                    _probability = _planeFinder.RansacProbability;
                    if (value != _probability) {
                        OnPropertyChanged("Probability");
                    }
                }
                _probability = value;
            } 
        }
        double _probability;

        /// <summary>
        /// Gets the multi-scan-once command, a one time kinect scan for a plane in point cloud.
        /// </summary>
        public ICommand MultiScanOnceCommand {
            get {
                if (_multiScanOnceCommand == null) {
                    _multiScanOnceCommand = new RelayCommand(obj => MultiScanAsync(), obj => !(_planeFinder == null || _isScanning));
                }
                return _multiScanOnceCommand;
            }
        }
        RelayCommand _multiScanOnceCommand;

        /// <summary>
        /// Gets the scan once command, a one time kinect scan for a plane in point cloud.
        /// </summary>
        public ICommand ScanOnceCommand {
            get {
                if (_scanOnceCommand == null) {
                    _scanOnceCommand = new RelayCommand(obj => ScanAsync(), obj => !(_planeFinder == null || _isScanning));
                }
                return _scanOnceCommand;
            }
        }
        RelayCommand _scanOnceCommand;

        #endregion


        #region Constructor(s)

        static MainWindowVM() {
            MATERIALS = new Material[8];
            MATERIALS[0] = MaterialHelper.CreateMaterial(Colors.Red);
            MATERIALS[1] = MaterialHelper.CreateMaterial(Colors.Firebrick);
            MATERIALS[2] = MaterialHelper.CreateMaterial(Colors.Maroon);
            MATERIALS[3] = MaterialHelper.CreateMaterial(Colors.DarkViolet);
            MATERIALS[4] = MaterialHelper.CreateMaterial(Colors.IndianRed);
            MATERIALS[5] = MaterialHelper.CreateMaterial(Colors.DeepPink);
            MATERIALS[6] = MaterialHelper.CreateMaterial(Colors.MediumOrchid);
            MATERIALS[7] = MaterialHelper.CreateMaterial(Colors.Crimson);
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="MainWindowVM"/> class, with
        /// default <c>Threshold</c> = 0.005 and <c>Probability</c> = 0.9.
        /// </summary>
        /// <param name="parent">The parent.</param>
        public MainWindowVM(MainWindow parent)
            : base() {
            _window = parent;
            _model3D = new Model3DGroup();
            State = ReadyState;
            Threshold = 0.01;
            Probability = 0.95;
            //Tiles = 1;
            Rows = 1;
            Columns = 1;
        }

        #endregion


        #region Methods

        protected override void Initialize() {
            base.Initialize();
            //_model3D = new Model3DGroup();
            _planeFinder = new PlaneFinder();
            _planeFinder.RansacThreshold = Threshold; // default 1 cm
            _planeFinder.RansacProbability = Probability; // default 95%
            _planeFinder.FrameRate = 0.5; // 1 Frame je 2 Sekunden
        }


        /// <summary>
        /// Asynchronously Kinect point cloud scan, that updates the <c>MainWindowVM.Model3D</c>.<para>
        /// Creates a visual 3D point cloud with colored points that belongs to the plane found by RANSAC.</para>
        /// </summary>
        async void ScanAsync() {
            _isScanning = true;
            State = ScanningState;
            PlaneDetectedEventArgs resu = await _planeFinder.ScanOnceAsync();

            if (resu == null) {
                setStateAndForceCmdUpdate(this.NoPlaneState);
                Model3D = new Model3DGroup(); // empty mesh
                return;
            }

            var kinectPCL = resu.PointCloud;
            Plane plane = resu.Plane;
            int[] planeIndexes = resu.Inliers;

            // KinectPoint to Windows.Media.Point to Mesh
            Model3DGroup newModel = new Model3DGroup();
            var meshBuilder = new MeshBuilder2(false, false);

            Point3D[] inliers;
            Point3D[] outliers;
            convertToInAndOutlierPoint3D(kinectPCL, planeIndexes, out inliers, out outliers);
            MeshGeometry3D mesh = CloudToMesh(inliers);
            newModel.Children.Add(new GeometryModel3D { Geometry = mesh, Material = REDMaterial, BackMaterial = SNOWWhiteMaterial });
            MeshGeometry3D mesh2 = CloudToMesh(outliers);
            newModel.Children.Add(new GeometryModel3D { Geometry = mesh2, Material = YELLOWMaterial, BackMaterial = SNOWWhiteMaterial });
            
            this.Model3D = newModel;
            setStateAndForceCmdUpdate(this.ReadyState);
        }

        /// <summary>
        /// Asynchronously Kinect zoned point cloud scan, that updates the <c>MainWindowVM.Model3D</c>.<para>
        /// Creates a visual 3D point cloud with colored points that belongs to the plane found by RANSAC.</para>
        /// </summary>
        async void MultiScanAsync() {
            _isScanning = true;
            State = ScanningState;
            PlaneDetectedEventArgs[] resu = await _planeFinder.ScanOnceAsync(Rows, Columns);

            if (resu == null) {
                setStateAndForceCmdUpdate(this.NoPlaneState);
                Model3D = new Model3DGroup(); // empty mesh
                return;
            }

            Model3DGroup newModel = new Model3DGroup();
            var meshBuilder = new MeshBuilder2(false, false);

            for (int i = 0; i < resu.Length; i++) {
                var kinectPCL = resu[i].PointCloud;
                int[] planeIndexes = resu[i].Inliers;
                Point3D[] inliers;
                Point3D[] outliers;
                convertToInAndOutlierPoint3D(kinectPCL, planeIndexes, out inliers, out outliers);
                MeshGeometry3D mesh = CloudToMesh(inliers);
                Material mat = MainWindowVM.MATERIALS[ i % MATERIALS.Length ];
                newModel.Children.Add(new GeometryModel3D { Geometry = mesh, Material = mat, BackMaterial = SNOWWhiteMaterial });
                MeshGeometry3D mesh2 = CloudToMesh(outliers);
                newModel.Children.Add(new GeometryModel3D { Geometry = mesh2, Material = YELLOWMaterial, BackMaterial = SNOWWhiteMaterial });
            }
            this.Model3D = newModel;
            setStateAndForceCmdUpdate(this.ReadyState);

        }

        /// <summary>
        /// Cloud to mesh of tetrahedons.
        /// </summary>
        /// <param name="points">The points.</param>
        /// <returns><c>3DMeshGeometry</c> with points converted to tetrahedons</returns>
        private MeshGeometry3D CloudToMesh(Point3D[] points) {
            var meshBuilder = new MeshBuilder2(false, false);
            for (int i = 0; i < points.Length; i++) {
                Point3D center = points[i];
                double sideLength = 0.005;
                //meshBuilder.AddBox(center, sideLength, sideLength, sideLength);
                
                // Tetraeder mit Grundfläche senkrecht nach hinten und Spitze zum
                // Kinectursrung, hin zu (0, 0, 0).
                Vector3D up = (center.ToVector3D() * -1.0);
                up.Normalize();
                var front = new Vector3D(0.0, 0.0, -1.0);

                meshBuilder.AddTetrahedon(center, front, up, sideLength, sideLength);
            }
            var meshResu = meshBuilder.ToMesh(true);
            return meshResu;
        }


        /// <summary>
        /// Converts to in and outlier point3 d.
        /// </summary>
        /// <param name="kinectPCL">The kinect PCL (not null).</param>
        /// <param name="planeIndexes">The plane indexes.</param>
        /// <param name="inliers">The inliers.</param>
        /// <param name="outliers">The outliers.</param>
        private static void convertToInAndOutlierPoint3D(CameraSpacePoint[] kinectPCL, int[] planeIndexes, out Point3D[] inliers, out Point3D[] outliers) {
            // Inlier Indizes sind nicht unbedingt sortiert!
            // Daher entweder pointcloud 2x durchlaufen (erst plane points kopieren und original auf
            // infinity setzen, dann non-planepoints kopieren) oder planeIndexes sortieren oder
            // laufenden index immer mit planeIndexes.contains prüfen
            if (planeIndexes == null) {
                inliers = new Point3D[0]; // leeres Array zurückgeben
            }
            else {
                Array.Sort<int>(planeIndexes); // Achtung! Sortiert auf Referenz (Originaldaten)
                inliers = new Point3D[planeIndexes.Length];
            }
            // Menge an outliers unbekannt, da +/- Unendlich Punkte nicht mitgezählt werden
            List<Point3D> outliersList = new List<Point3D>(kinectPCL.Length);
            int inlierIdx = 0; // kinectPCL Inlier Index
            for (int i = 0; i < kinectPCL.Length; i++) {
                CameraSpacePoint kp = kinectPCL[i];
                if (float.IsInfinity(kp.Z))
                    continue;
                if (float.IsInfinity(kp.Y))
                    continue;
                if (float.IsInfinity(kp.X))
                    continue;

                //Point3D p = new Point3D { X = -kp.Z, Y = kp.X, Z = kp.Y };
                // KinectBild ist gespiegelt, daher Horizontale negieren
                Point3D p = new Point3D { X = -kp.Z, Y = -kp.X, Z = kp.Y };
                if (inliers.Length != 0 && i == planeIndexes[inlierIdx]) {
                    // Punkt gehört zur Fläche: einfügen und Array Index hochzählen
                    inliers[inlierIdx] = p;
                    if (inlierIdx + 1 != inliers.Length) {
                        inlierIdx++;
                    }
                    else {
                        inlierIdx = inlierIdx + 1 - 1;
                        continue;
                    }
                }
                else {
                    // dann gehört der Punkt wohl zu den Ausreißern
                    outliersList.Add(p);
                }
            }

            outliers = outliersList.ToArray();
        }

        
        /// <summary>
        /// Sets the specified state, _isScanning to false and force command update.
        /// </summary>
        /// <param name="state">The state.</param>
        void setStateAndForceCmdUpdate(string state) {
            State = state;
            _isScanning = false;
            // Forcing the CommandManager to raise the RequerySuggested event
            CommandManager.InvalidateRequerySuggested();
        }



        /// <summary>
        /// Stop PlaneFinder 
        /// </summary>
        protected override void OnDispose() {
            base.OnDispose();
            _model3D = null;
            if (_planeFinder != null) {
                _planeFinder.Stop();
            }
        }

        #endregion
    }


}
